#include <reg51.h>
#include "delay.h"

sfr AUXR = 0x8E;
sbit Trig = P2^1;
sbit Echo = P2^0;

#define SPEED  0.34  //  mm/us


void Echo_Init() 
{
	Trig = 0;
	Echo = 0;
	AUXR &= 0x7F;	
	TMOD &= 0xF0;
	TMOD |= 0x01; //Timer0  16bit
	TL0 = 0x00;
	TH0 = 0x00;	
}

float Echo_Measure_CM() 
{
	unsigned int count_us = 0;
	float distance_mm  = 0.0;
	
	TL0 = 0x00;
	TH0 = 0x00;
	Echo = 0;	
	
	Trig = 1;
	delay(2);
	Trig = 0;
	
	while(Echo == 0) {}
	TR0 = 1;
	while(Echo == 1) {}
	TR0 = 0;
		
	count_us = (((unsigned int)TH0) << 8) | (unsigned int)TL0;
	distance_mm = count_us * SPEED;
	return distance_mm * 0.05;
}